/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   This is the main file for the UAVs autopilots. It is based on open source projects:
*   do it yourself autpilot and MNAV navigation from XBOW.
*	The main file describes the mltithreads structure and machine-state implementation.
*
* SOURCE: 
* Original: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio Garcia-Nieto sergarro@isa.upv.es
* $Id: flightComputer.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/

//----------------------------------------------- 
// Includes
//----------------------------------------------- 

// System Includes
//----------------------------------------------- 
#include <sched.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <pthread.h>
#include <setjmp.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <unistd.h>
#include <sys/neutrino.h>
#include <time.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <termios.h>

// Function Includes
//----------------------------------------------- 
#include "adc.h"
#include "globaldefs.h"
#include "utilities.h"

// Threads Includes
//----------------------------------------------- 
#include "attitudeCtrl_thread.h"
#include "cameraCtrl_thread.h"
#include "datalogging_thread.h"
#include "healthMntr_thread.h"
#include "idle_thread.h"
#include "navigationCtrl_thread.h"
#include "telemetry_thread.h"
#include "watchdog_thread.h"

//----------------------------------------------- 
// Main Thread: State Machine
//----------------------------------------------- 

int main(int argc, char *argv[]) {
	
	//----------------------------------------------- 
	// Local Variables in Main Thread 
	//----------------------------------------------- 
	int pol;
	int i;
	char option, option_1;
	struct timespec t0,t1,t0_ant,wait,next_activation;
	struct _clockperiod periodoreloj;
	_uint64 auxtime;
	//---------------------------------------------- 
	// WayPoints Initialization
	//---------------------------------------------- 
	for (i=0;i<MAX_WAYPOINTS;i++)
	{
		flightPlan[i].numPoint = i;
		flightPlan[i].latRef = 0;
		flightPlan[i].lonRef = 0;
		flightPlan[i].altRef = 0;
		flightPlan[i].velRef = 0;
	};
	//---------------------------------------------- 
	// Threads' Initialization
	//---------------------------------------------- 
	periodoreloj.nsec = 100000;
	ClockPeriod(CLOCK_REALTIME,&periodoreloj,NULL,0);
	
	ID_stateMachine = NULL;
	ID_attitudeCtrl = NULL;
	ID_navigationCtrl = NULL;
	ID_cameraCtrl = NULL;
	ID_telemetry = NULL;
	
	pthread_mutex_unlock(&mutexPidLowLevel);
	pthread_mutex_unlock(&mutexPidHighLevel);
	pthread_mutex_unlock(&mutexGlobalVar);
	pthread_mutex_unlock(&mutexMachineState);
	// Mutex Initialization
		/*pthread_mutexattr_init (&at);
		pthread_mutexattr_settype (&at, PTHREAD_MUTEX_RECURSIVE);
		pthread_mutexattr_setprotocol (&at, PTHREAD_PRIO_PROTECT);
		pthread_mutex_init (&mutexPid, &at);
		pthread_mutex_setprioceiling (&mutexPid,1,0); //priority ceiling a 1
		//pthread_mutex_setprioceiling (&m2,1,0); //priority ceiling a 1 */

	
	// Scheduling of the tasks
		// state machine
		pthread_getschedparam(pthread_self(),&pol,&param);//attr_main); // Scheduling of the Taks Main = stateMachine
		pthread_attr_getschedparam(&attr_stateMachine,&param);
		pthread_attr_setdetachstate (&attr_stateMachine, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_stateMachine, SCHED_FIFO);
		// idle task
		pthread_attr_init(&attr_idle);
		pthread_attr_setdetachstate (&attr_idle, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_idle, SCHED_FIFO);
		// attitude task
		pthread_attr_init(&attr_attitudeCtrl);
		pthread_attr_setdetachstate (&attr_attitudeCtrl, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_attitudeCtrl, SCHED_FIFO);
		// navigation task
		pthread_attr_init(&attr_navigationCtrl);
		pthread_attr_setdetachstate (&attr_navigationCtrl, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_navigationCtrl, SCHED_FIFO);
		// camera task (not using yet)
		pthread_attr_init(&attr_cameraCtrl);
		pthread_attr_setdetachstate (&attr_cameraCtrl, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_cameraCtrl, SCHED_FIFO);
		// health task (not using yet)
		pthread_attr_init(&attr_healthMntr);
		pthread_attr_setdetachstate (&attr_healthMntr, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_healthMntr, SCHED_FIFO);
		// telemetry task
		pthread_attr_init(&attr_telemetry);
		pthread_attr_setdetachstate (&attr_telemetry, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_telemetry, SCHED_FIFO);
		// datalogging task
		pthread_attr_init(&attr_datalogging);
		pthread_attr_setdetachstate (&attr_datalogging, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_datalogging, SCHED_FIFO);
		// watchdog task (not using yet)
		pthread_attr_init(&attr_watchdog);
		pthread_attr_setdetachstate (&attr_watchdog, PTHREAD_CREATE_DETACHED);
		pthread_attr_setschedpolicy(&attr_watchdog, SCHED_FIFO);
		
	//  Tasks' Priority
		// state machine
		pthread_attr_setinheritsched (&attr_stateMachine, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 200;
		pthread_attr_setschedparam(&attr_stateMachine,&param);
		pthread_setschedparam(pthread_self(),SCHED_FIFO,&param);// Priority of the Main Fucntion = stateMachine
		// attitude task
		pthread_attr_setinheritsched (&attr_attitudeCtrl, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 255;
		pthread_attr_setschedparam(&attr_attitudeCtrl,&param);
		// idle task
		pthread_attr_setinheritsched (&attr_idle, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 250;
		pthread_attr_setschedparam(&attr_idle,&param);
		// navigation task
		pthread_attr_setinheritsched (&attr_navigationCtrl, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 250;
		pthread_attr_setschedparam(&attr_navigationCtrl,&param);
		// camera task
		pthread_attr_setinheritsched (&attr_cameraCtrl, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 210;
		pthread_attr_setschedparam(&attr_cameraCtrl,&param);
		// health task
		pthread_attr_setinheritsched (&attr_healthMntr, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 5;
		pthread_attr_setschedparam(&attr_healthMntr,&param);
		// telemetry task
		pthread_attr_setinheritsched (&attr_telemetry, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 240;
		pthread_attr_setschedparam(&attr_telemetry,&param);
		// datalogging task
		pthread_attr_setinheritsched (&attr_datalogging, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 230;
		pthread_attr_setschedparam(&attr_datalogging,&param);
		// watchdog task
		pthread_attr_setinheritsched (&attr_watchdog, PTHREAD_EXPLICIT_SCHED);
		param.sched_priority = 25;
		pthread_attr_setschedparam(&attr_watchdog,&param);
		
	// Tasks' Period in nanoseconds
		// satate machine
		Ts_stateMachine.tv_sec=1;
		Ts_stateMachine.tv_nsec=00000000; // 0.5 Hz
		// idle task
		Ts_idle.tv_sec=0;
		Ts_idle.tv_nsec=250000000; // 4 Hz
		// attitude task
		Ts_attitudeCtrl.tv_sec=0;
		Ts_attitudeCtrl.tv_nsec=20000000; // 20 Hz
		// navigation task
		Ts_navigationCtrl.tv_sec=0;
		Ts_navigationCtrl.tv_nsec=250000000;// 4 Hz
		// camera task
		Ts_cameraCtrl.tv_sec=1;
		Ts_cameraCtrl.tv_nsec=00000000; // 1 Hz
		// telemetry task
		Ts_telemetry.tv_sec=0;
		Ts_telemetry.tv_nsec=500000000; // 2 Hz
		// heath task
		Ts_healthMntr.tv_sec=1;
		Ts_healthMntr.tv_nsec=00000000; // 1 Hz
		// datalogging task
		Ts_datalogging.tv_sec=1;
		Ts_datalogging.tv_nsec=000000000; // 0.1 Hz
		// watchdog task
		Ts_watchdog.tv_sec=5;
		Ts_watchdog.tv_nsec=0000000; // 1 Hz
		
<<<<<<< .mine
		//-----------------------------------------------
		// PIDs High Level Initialization
		//-----------------------------------------------
		pthread_mutex_lock(&mutexPidHighLevel);
		// PID Altitude Initial Parameters
		pidAltitude.Signals.R=0;
		pidAltitude.Signals.y=0;
		pidAltitude.Signals.u=0;
				
		pidAltitude.States.R1=0;
		pidAltitude.States.R2=0;
		pidAltitude.States.R3=0;
		pidAltitude.States.e=0;
		pidAltitude.States.e_1=0;
		pidAltitude.States.e_2=0;
		pidAltitude.States.u_1=0;
		pidAltitude.States.I=0;
		pidAltitude.States.D=0;
							
		pidAltitude.Par.K=0.04;
		pidAltitude.Par.Td=0;
		pidAltitude.Par.Ti=0;
		pidAltitude.Par.ulow=-0.25;
		pidAltitude.Par.uhigh=0.25;
		pidAltitude.Par.Ts=0.25;
		pidAltitude.Par.u0=0.00;
				
				
		// PID Heading Initial Parameters
		pidHeading.Signals.R=0;
		pidHeading.Signals.y=0;
		pidHeading.Signals.u=0;
				
		pidHeading.States.R1=0;
		pidHeading.States.R2=0;
		pidHeading.States.R3=0;
		pidHeading.States.e=0;
		pidHeading.States.e_1=0;
		pidHeading.States.e_2=0;
		pidHeading.States.u_1=0;
		pidHeading.States.I=0;
		pidHeading.States.D=0;
				
		pidHeading.Par.K=0.5;
		pidHeading.Par.Td=0;
		pidHeading.Par.Ti=0;
		pidHeading.Par.ulow=-0.25;
		pidHeading.Par.uhigh=0.25;
		pidHeading.Par.Ts=0.25;
		pidHeading.Par.u0=0;
				
		// PID Velocity Initial Parameters
		pidVelocity.Signals.R=0;
		pidVelocity.Signals.y=0;
		pidVelocity.Signals.u=0;
				
		pidVelocity.States.R1=0;
		pidVelocity.States.R2=0;
		pidVelocity.States.R3=0;
		pidVelocity.States.e=0;
		pidVelocity.States.e_1=0;
		pidVelocity.States.e_2=0;
		pidVelocity.States.u_1=0;
		pidVelocity.States.I=0;
		pidVelocity.States.D=0;
				
		pidVelocity.Par.K=0.5;
		pidVelocity.Par.Td=0;
		pidVelocity.Par.Ti=0.1;
		pidVelocity.Par.ulow=-20;
		pidVelocity.Par.uhigh=20;
		pidVelocity.Par.Ts=0.25;
		pidVelocity.Par.u0=0;
		pthread_mutex_unlock(&mutexPidHighLevel);
			
		//-----------------------------------------------
		// PIDs Low level Initialization
		//-----------------------------------------------
		pthread_mutex_lock(&mutexPidLowLevel);
		// PID Roll Initial Parameters
		pidRoll.Signals.R=0;
		pidRoll.Signals.y=0;
		pidRoll.Signals.u=0;
				
		pidRoll.States.R1=0;
		pidRoll.States.R2=0;
		pidRoll.States.R3=0;
		pidRoll.States.e=0;
		pidRoll.States.e_1=0;
		pidRoll.States.e_2=0;
		pidRoll.States.u_1=0;
		pidRoll.States.I=0;
		pidRoll.States.D=0;
				
					
		pidRoll.Par.K=0.5;
		pidRoll.Par.Td=0;
		pidRoll.Par.Ti=10;
		pidRoll.Par.ulow=-0.15;
		pidRoll.Par.uhigh=0.15;
		pidRoll.Par.Ts=0.02;
		pidRoll.Par.u0=0;
					
		// PID Pitch Initial Parameters
		pidPitch.Signals.R=0;
		pidPitch.Signals.y=0;
		pidPitch.Signals.u=0;
				
		pidPitch.States.R1=0;
		pidPitch.States.R2=0;
		pidPitch.States.R3=0;
		pidPitch.States.e=0;
		pidPitch.States.e_1=0;
		pidPitch.States.e_2=0;
		pidPitch.States.u_1=0;
		pidPitch.States.I=0;
		pidPitch.States.D=0;
					
		pidPitch.Par.K=0.5;
		pidPitch.Par.Td=0;
		pidPitch.Par.Ti=0.1;
		pidPitch.Par.ulow=-0.15;
		pidPitch.Par.uhigh=0.15;
		pidPitch.Par.Ts=0.02;
		pidPitch.Par.u0=0;
						
		// PID Yaw Initial Parameters
		pidYaw.Signals.R=0;
		pidYaw.Signals.y=0;
		pidYaw.Signals.u=0;
				
		pidYaw.States.R1=0;
		pidYaw.States.R2=0;
		pidYaw.States.R3=0;
		pidYaw.States.e=0;
		pidYaw.States.e_1=0;
		pidYaw.States.e_2=0;
		pidYaw.States.u_1=0;
		pidYaw.States.I=0;
		pidYaw.States.D=0;
					
		pidYaw.Par.K=1;
		pidYaw.Par.Td=0;
		pidYaw.Par.Ti=0;
		pidYaw.Par.ulow=-0.35;
		pidYaw.Par.uhigh=0.35;
		pidYaw.Par.Ts=0.02;
		pidYaw.Par.u0=0;
		pthread_mutex_unlock(&mutexPidLowLevel);
			
		// FlightPlan Initialization	
		pthread_mutex_lock(&mutexGlobalVar);
		semiAutomatic = 0;
		totalWayPoints = 0;
		actualWayPoint = 0;
		pthread_mutex_unlock(&mutexGlobalVar);	
			
		// Adc Initialization
		adcInitialization();
=======
		//-----------------------------------------------
		// PIDs High Level Initialization
		//-----------------------------------------------
		pthread_mutex_lock(&mutexPidHighLevel);
		// PID Altitude Initial Parameters
		pidAltitude.Signals.R=0;
		pidAltitude.Signals.y=0;
		pidAltitude.Signals.u=0;
				
		pidAltitude.States.R1=0;
		pidAltitude.States.R2=0;
		pidAltitude.States.R3=0;
		pidAltitude.States.e=0;
		pidAltitude.States.e_1=0;
		pidAltitude.States.e_2=0;
		pidAltitude.States.u_1=0;
		pidAltitude.States.I=0;
		pidAltitude.States.D=0;
							
		pidAltitude.Par.K=0.04;
		pidAltitude.Par.Td=0;
		pidAltitude.Par.Ti=0;
		pidAltitude.Par.ulow=-0.25;
		pidAltitude.Par.uhigh=0.25;
		pidAltitude.Par.Ts=0.25;
		pidAltitude.Par.u0=0.05;
				
				
		// PID Heading Initial Parameters
		pidHeading.Signals.R=0;
		pidHeading.Signals.y=0;
		pidHeading.Signals.u=0;
				
		pidHeading.States.R1=0;
		pidHeading.States.R2=0;
		pidHeading.States.R3=0;
		pidHeading.States.e=0;
		pidHeading.States.e_1=0;
		pidHeading.States.e_2=0;
		pidHeading.States.u_1=0;
		pidHeading.States.I=0;
		pidHeading.States.D=0;
				
		pidHeading.Par.K=0.5;
		pidHeading.Par.Td=0;
		pidHeading.Par.Ti=0;
		pidHeading.Par.ulow=-0.25;
		pidHeading.Par.uhigh=0.25;
		pidHeading.Par.Ts=0.25;
		pidHeading.Par.u0=0;
				
		// PID Velocity Initial Parameters
		pidVelocity.Signals.R=0;
		pidVelocity.Signals.y=0;
		pidVelocity.Signals.u=0;
				
		pidVelocity.States.R1=0;
		pidVelocity.States.R2=0;
		pidVelocity.States.R3=0;
		pidVelocity.States.e=0;
		pidVelocity.States.e_1=0;
		pidVelocity.States.e_2=0;
		pidVelocity.States.u_1=0;
		pidVelocity.States.I=0;
		pidVelocity.States.D=0;
				
		pidVelocity.Par.K=0.5;
		pidVelocity.Par.Td=0;
		pidVelocity.Par.Ti=0.1;
		pidVelocity.Par.ulow=-20;
		pidVelocity.Par.uhigh=20;
		pidVelocity.Par.Ts=0.25;
		pidVelocity.Par.u0=0;
		pthread_mutex_unlock(&mutexPidHighLevel);
			
		//-----------------------------------------------
		// PIDs Low level Initialization
		//-----------------------------------------------
		pthread_mutex_lock(&mutexPidLowLevel);
		// PID Roll Initial Parameters
		pidRoll.Signals.R=0;
		pidRoll.Signals.y=0;
		pidRoll.Signals.u=0;
				
		pidRoll.States.R1=0;
		pidRoll.States.R2=0;
		pidRoll.States.R3=0;
		pidRoll.States.e=0;
		pidRoll.States.e_1=0;
		pidRoll.States.e_2=0;
		pidRoll.States.u_1=0;
		pidRoll.States.I=0;
		pidRoll.States.D=0;
				
					
		pidRoll.Par.K=0.5;
		pidRoll.Par.Td=0;
		pidRoll.Par.Ti=10;
		pidRoll.Par.ulow=-0.25;
		pidRoll.Par.uhigh=0.25;
		pidRoll.Par.Ts=0.02;
		pidRoll.Par.u0=0;
					
		// PID Pitch Initial Parameters
		pidPitch.Signals.R=0;
		pidPitch.Signals.y=0;
		pidPitch.Signals.u=0;
				
		pidPitch.States.R1=0;
		pidPitch.States.R2=0;
		pidPitch.States.R3=0;
		pidPitch.States.e=0;
		pidPitch.States.e_1=0;
		pidPitch.States.e_2=0;
		pidPitch.States.u_1=0;
		pidPitch.States.I=0;
		pidPitch.States.D=0;
					
		pidPitch.Par.K=0.5;
		pidPitch.Par.Td=0;
		pidPitch.Par.Ti=0.1;
		pidPitch.Par.ulow=-0.35;
		pidPitch.Par.uhigh=0.35;
		pidPitch.Par.Ts=0.02;
		pidPitch.Par.u0=0;
						
		// PID Yaw Initial Parameters
		pidYaw.Signals.R=0;
		pidYaw.Signals.y=0;
		pidYaw.Signals.u=0;
				
		pidYaw.States.R1=0;
		pidYaw.States.R2=0;
		pidYaw.States.R3=0;
		pidYaw.States.e=0;
		pidYaw.States.e_1=0;
		pidYaw.States.e_2=0;
		pidYaw.States.u_1=0;
		pidYaw.States.I=0;
		pidYaw.States.D=0;
					
		pidYaw.Par.K=1;
		pidYaw.Par.Td=0;
		pidYaw.Par.Ti=0;
		pidYaw.Par.ulow=-0.35;
		pidYaw.Par.uhigh=0.35;
		pidYaw.Par.Ts=0.02;
		pidYaw.Par.u0=0;
		pthread_mutex_unlock(&mutexPidLowLevel);
			
		// FlightPlan Initialization	
		pthread_mutex_lock(&mutexGlobalVar);
		semiAutomatic = 0;
		totalWayPoints = 0;
		actualWayPoint = 0;
		pthread_mutex_unlock(&mutexGlobalVar);	
			
		// Adc Initialization
		adcInitialization();
>>>>>>> .r976
	
	//----------------------------------------------- 
	// State Machine Initialization
	//-----------------------------------------------
		pthread_mutex_lock(&mutexMachineState);
		X1_idle = 1;
		X2_takeoff = 0;
		X3_navigation = 0;
		X4_landing = 0;
		
		
		event_0 = 0;
		event_1 = 0;
		event_2 = 0;
		event_3 = 0;
		event_4 = 0;
		event_5 = 0;
		pthread_mutex_unlock(&mutexMachineState);
		
		option_1 = 0;
		
		clock_gettime (CLOCK_REALTIME, &t0_ant);
		clock_gettime (CLOCK_REALTIME, &t1);
		
	//----------------------------------------------- 
	// State Machine Code
	//----------------------------------------------- 	
		while(1) 
		{
		// Loop time claculations
			clock_gettime (CLOCK_REALTIME, &t0);
			next_activation=sum_tiempos(t0,Ts_stateMachine); 
			
		// Outputs of the State Machine
		option = (X4_landing << 3)+(X3_navigation << 2)+(X2_takeoff<<1)+X1_idle;
		//printf("%d\n",option);	
		if (option!=option_1)
		{
			// Assign Variable

										
				switch(option)
				{
			   		case 1:
			   		//  Threads and actions in state X1_idle
					//--------------------------------------------------------
						printf("Starting Tasks from state Idle...\n");
					
					// End Task Tasks
						if (ID_attitudeCtrl!=NULL)
							ThreadDestroy(ID_attitudeCtrl,-1,NULL);
						if (ID_navigationCtrl!=NULL)
							ThreadDestroy(ID_navigationCtrl,-1,NULL);
						if (ID_datalogging!=NULL)
							ThreadDestroy(ID_datalogging,-1,NULL);
					//Initilization Task 
						pthread_create (&ID_idle, &attr_idle, idle, 0);
					//Telemetry Task
						if (ID_telemetry==NULL)
							pthread_create (&ID_telemetry, &attr_telemetry, telemetry, 0);
						
						printf ("All the Tasks have been started.\n");
						printf("-----------------------------------------------------------------------\n");
			    		
			   		break;
			  		case 2:
			   		//  Threads and actions in state X2_takeoff
					//--------------------------------------------------------
						printf("Starting Tasks from state Takeoff...\n");
						printf ("All the Tasks have been started.\n");
						printf("-----------------------------------------------------------------------\n");

										    		
			   		break;
					case 4: 
					//  Threads and actions in state X3_navigation
					//--------------------------------------------------------					
						printf("Starting Tasks from state Navigation...\n");
					// End Task Tasks
					if (ID_idle!=NULL)
						ThreadDestroy(ID_idle,-1,NULL);

					//Attitude Task 
						pthread_create (&ID_attitudeCtrl, &attr_attitudeCtrl, attitudeCtrl, 0);
					//Navigation Control Task
						pthread_create (&ID_navigationCtrl, &attr_navigationCtrl, navigationCtrl, 0);

						//Health Management Task
						//pthread_create (&ID_healthMntr, &attr_healthMntr, healthMntr, 0);
					//Camera Control Task
						//pthread_create (&ID_cameraCtrl, &attr_cameraCtrl, cameraCtrl, 0);
					//Datalogging Control Task
						pthread_create (&ID_datalogging, &attr_datalogging, datalogging, 0);
			
						printf ("All the Tasks have been started.\n");
						printf("-----------------------------------------------------------------------\n");
						
					break;
					case 8:
					//  Threads and actions in state X4_landing
					//--------------------------------------------------------					
						printf("Starting Tasks from state Takeoff...\n");
						printf ("All the Tasks have been started.\n");
						printf("-----------------------------------------------------------------------\n");

					break;
			      default:

			      break;
			   	}
		}	
		option_1 = option;
			//FLIP Equation
		
			//X1_idle = X1_idle*(!(X2_takeoff+X3_navigation))+X4_landing*event_5+X3_navigation*event_3;
			//X2_takeoff = X1_idle*event_0+X2_takeoff*(!X3_navigation);
			//X3_navigation = X2_takeoff*event_1+X1_idle*event_2+X3_navigation*(!X1_idle);
			//X4_landing = X3_navigation*event_4+X4_landing*(!X1_idle);

			//FLIP-FLOP Equation
	        pthread_mutex_lock(&mutexMachineState);
			X1_idle = X1_idle*(!X3_navigation)+X4_landing*event_5+X3_navigation*event_3;
			X2_takeoff = X1_idle*event_0+X2_takeoff*(!X3_navigation);
			X3_navigation = X2_takeoff*event_1+X1_idle*event_2+X3_navigation*(!X1_idle);
			X4_landing = X3_navigation*event_4+X4_landing*(!X1_idle);
			pthread_mutex_unlock(&mutexMachineState);
			// Print Results
			//if(v) printf("Inicialization--> WCCT: %3d Period: %3d\n",((int)WCCT.tv_nsec)/prec,  ((int)t0.tv_nsec-(int)t0_ant.tv_nsec)/prec);
			
			//if(v) printf("sM %3f %3f \n",((double)WCCT.tv_nsec)/prec,  ((double)Period.tv_nsec)/prec);
			// Wait until the next activation
			clock_gettime (CLOCK_REALTIME, &t1);
			if (timespec2nsec(&next_activation)>timespec2nsec(&t1))
			{
			    auxtime = (timespec2nsec(&next_activation)-timespec2nsec(&t1));
				nsec2timespec(&wait,auxtime);
				nanosleep(&wait, NULL);
			}	
		}	
	return EXIT_SUCCESS;
}


